#include "sbus_set_G4.h"

SBUS_t sbus_ch;  
// 双缓冲区
static uint8_t sbus_rx_buf[LD_SBUS_RX_BUF_NUM];   // 环形缓冲区，长度为2帧数据大小
static uint16_t sbus_last_pos=0;               // 上次处理数据位置

/**
************************************************************************
* @brief:      	初始化SBUS接收，开启双缓冲DMA接收
* @param[in]:   huart: UART句柄指针
* @retval:     	void
* @details:    	配置UART的DMA双缓冲接收，并开启IDLE中断
************************************************************************
**/
void SBUS_Init(UART_HandleTypeDef *huart)
{
	huart->ReceptionType = HAL_UART_RECEPTION_TOIDLE;
	huart->RxEventType = HAL_UART_RXEVENT_IDLE;
	huart->RxXferSize  = LD_SBUS_RX_BUF_NUM;

	SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
	__HAL_UART_ENABLE_IT(huart, UART_IT_IDLE);

	// 启动DMA接收，环形模式（在CubeMX配置DMA时选环形）
	HAL_UARTEx_ReceiveToIdle_DMA(huart, sbus_rx_buf, LD_SBUS_RX_BUF_NUM);

	sbus_last_pos = 0;
}

/**
************************************************************************
* @brief:      	rx_set: 根据手册对遥控器接收数据进行解算
* @param[in]:   sbus_rx_buf:		接收数据存储的数组变量
* @param[in]:   sbus: 		指向SBUS_t结构的指针
* @retval:     	void
* @details:    	接收串口接收返回数据
************************************************************************
**/
void rx_set(uint8_t *sbus_rx_buf,SBUS_t *sbus)
{
	uint8_t cnt=0;
	if(sbus_rx_buf[23] == 0x00 && sbus_rx_buf[0] == 0x0f && sbus_rx_buf[24] == 0x00)
	{
		sbus->ch[0] =((sbus_rx_buf[2]<<8)   + (sbus_rx_buf[1])) & 0x07ff;
		sbus->ch[1] =((sbus_rx_buf[3]<<5)   + (sbus_rx_buf[2]>>3)) & 0x07ff;
		sbus->ch[2] =((sbus_rx_buf[5]<<10)  + (sbus_rx_buf[4]<<2) + (sbus_rx_buf[3]>>6)) & 0x07ff;
		sbus->ch[3] =((sbus_rx_buf[6]<<7)   + (sbus_rx_buf[5]>>1)) & 0x07ff;
		sbus->ch[4] =((sbus_rx_buf[7]<<4)   + (sbus_rx_buf[6]>>4)) & 0x07ff;
		sbus->ch[5] =((sbus_rx_buf[9]<<9)   + (sbus_rx_buf[8]<<1) + (sbus_rx_buf[7]>>7)) & 0x07ff;
		sbus->ch[6] =((sbus_rx_buf[10]<<6)  + (sbus_rx_buf[9]>>2)) & 0x07ff;
		sbus->ch[7] =((sbus_rx_buf[11]<<3)  + (sbus_rx_buf[10]>>5)) & 0x07ff;
		sbus->ch[8] =((sbus_rx_buf[13]<<8)  + (sbus_rx_buf[12])) & 0x07ff;
		sbus->ch[9] =((sbus_rx_buf[14]<<5)  + (sbus_rx_buf[13]>>3)) & 0x07ff;
		sbus->ch[10]=((sbus_rx_buf[16]<<10) + (sbus_rx_buf[15]<<2) + (sbus_rx_buf[14]>>6)) & 0x07ff;
		sbus->ch[11]=((sbus_rx_buf[17]<<7)  + (sbus_rx_buf[16]>>1)) & 0x07ff;
		sbus->ch[12]=((sbus_rx_buf[18]<<4)  + (sbus_rx_buf[17]>>4)) & 0x07ff;
		sbus->ch[13]=((sbus_rx_buf[20]<<9)  + (sbus_rx_buf[19]<<1) + (sbus_rx_buf[18]>>7)) & 0x07ff;
		sbus->ch[14]=((sbus_rx_buf[21]<<6)  + (sbus_rx_buf[20]>>2)) & 0x07ff;
		sbus->ch[15]=((sbus_rx_buf[22]<<3)  + (sbus_rx_buf[21]>>5)) & 0x07ff;
		
		sbus->ch[0] -= SBUS_CH_VALUE_OFFSET;
		sbus->ch[1] -= SBUS_CH_VALUE_OFFSET;
		sbus->ch[2] -= SBUS_CH_VALUE_OFFSET;
		sbus->ch[3] -= SBUS_CH_VALUE_OFFSET;
		
		//死区控制，防止遥控器数据波动
		for(int i=0;i<4;i++)
		{
			if((sbus->ch[i]>0&&sbus->ch[i]<20)||(sbus->ch[i]<0&&sbus->ch[i]>-20))
			{
				sbus->ch[i]=0;
			}
		}
		//拨杆简化
		for(int i=0;i<4;i++)
		{
			switch(sbus->ch[i+4])
			{
				case sbus_s_1:sbus->s[i]=1;break;
				case sbus_s_2:sbus->s[i]=2;break;
				case sbus_s_3:sbus->s[i]=3;break;
				default:break;
			}
		}
		for(int i=0;i<2;i++)
		{
			sbus->knob[i]=sbus->ch[i+8]-sbus_knob_zero;
		}
	}
	else
	{
		HAL_UART_DMAStop(&huart3);
		for(cnt=0;cnt<25;cnt++)
		{
			sbus_rx_buf[cnt]=0;
		}
		for(cnt=0;cnt<25;cnt++)
		{
			sbus->ch[cnt]=0;
		}
	}
}

/**
************************************************************************
* @brief:      	接收完成后的数据处理函数
* @param[in]:   huart: UART句柄
* @param[in]:   Size:  本次接收到数据的长度
* @retval:     	void
* @details:    	通过软件变量轮流处理双缓冲区数据
************************************************************************
**/
void SBUS_RxHandler(UART_HandleTypeDef *huart, uint16_t Size)
{
	uint16_t pos = LD_SBUS_RX_BUF_NUM-__HAL_DMA_GET_COUNTER(huart->hdmarx);  // 当前DMA写到哪个位置
	uint16_t data_len;

	// 处理缓冲区中新增的完整帧
	while(pos != sbus_last_pos)
	{
		if(sbus_last_pos + LD_RC_FRAME_LENGTH <= LD_SBUS_RX_BUF_NUM)
		{
			data_len = LD_RC_FRAME_LENGTH;
		}
		else
		{
			// 如果剩余空间不足一帧，拆成两部分处理
			data_len = LD_SBUS_RX_BUF_NUM - sbus_last_pos;
		}

		if(data_len == LD_RC_FRAME_LENGTH)
		{
			rx_set(&sbus_rx_buf[sbus_last_pos], &sbus_ch);
		}

		sbus_last_pos += LD_RC_FRAME_LENGTH;
		if (sbus_last_pos >= LD_SBUS_RX_BUF_NUM)
				sbus_last_pos = 0;
	}
}

/**
************************************************************************
* @brief:      	HAL UART IDLE 中断回调
* @param[in]:   huart: UART句柄
* @param[in]:   Size: 本次接收长度
* @retval:     	void
* @details:    	调用自定义处理函数
************************************************************************
**/
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart,uint16_t Size) 
{ 
    if(huart==&huart3)
    { 
      SBUS_RxHandler(huart, Size); 
    }  
}
